#include <v8_system.h>
#include <gicv3_reg.h>

    .section ".text.boot"

    .global _start
    .global stack_top

_start:
    mrs x1, mpidr_el1
    and x1, x1, #3
    cbz x1, cpu0_start

secondary_cpu_deadloop:
    wfe
    b secondary_cpu_deadloop

cpu0_start:
    mrs x0, CurrentEL

    ldr x1, =el1_vectors
    msr VBAR_EL1, x1

    ldr x1, =el3_vectors
    msr VBAR_EL3, x1

    mov x3, #(SCR_EL3_RW)
    msr SCR_EL3, x3
    isb

    adr x1, el1_entry_aarch64
    msr ELR_EL3, x1
    mov x1, #(AARCH64_SPSR_EL1h | \
              AARCH64_SPSR_F  | \
              AARCH64_SPSR_I  | \
              AARCH64_SPSR_A)
    msr SPSR_EL3, x1
    eret

    .global el1_entry_aarch64
    .type el1_entry_aarch64, "function"

el1_entry_aarch64:
    mrs x0, CurrentEL

    /* clear bss section */
    ldr x1, =__bss_start
    ldr w2, =__bss_size
bss_clear:
    cbz w2, c_entry
    str xzr, [x1], #8
    sub w2, w2, #1
    cbnz w2, bss_clear

c_entry:

    msr spsel, #0
    ldr x0, =stack_top
    mov sp, x0

    bl main

deadloop:
    b deadloop